/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    CANJaguar jag1;
    //RobotDrive orange;
    
    public void robotInit() {
        try {
            jag1 = new CANJaguar(3);
            jag1.changeControlMode(CANJaguar.ControlMode.kSpeed);
            jag1.setPID(.3, .001, 0);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        
        //orange = new RobotDrive(jag1, jag1);
        //orange.setSafetyEnabled(false);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        
        try {
            jag1.enableControl();
            jag1.setX(200);
            jag1.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
            
            System.out.println("Current Speed is " + jag1.getSpeed());
            System.out.println("Current RPM is " + jag1.getX());
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
    }
    
}
